import sys, json, math
sys.path.append("/home/orangepi/shufly/src/pi_car/scripts/")

import numpy as np

import rospy
from std_msgs.msg import Float32

from hardware.Motor import Motor
from control.pid import PID
from pi_car.srv import String, StringRequest, StringResponse
from std_srvs.srv import Empty, EmptyRequest, EmptyResponse

from nlink_parser.msg import LinktrackNodeframe2

class ControlNode:
    MAX_STEER = 0.2
    MAX_THRO = 0.4

    def __init__(self) -> None:
        #----------Hardware--------------------------
        self.motor_left = Motor(4, 0, ("/dev/gpiochip0", 29, "out"), ("/dev/gpiochip4", 11, "out"))
        self.motor_right = Motor(0, 0, ("/dev/gpiochip1", 18, "out"), ("/dev/gpiochip1", 20, "out"))

        self.motor_left.frequency = 500
        self.motor_right.frequency = 500

        self.motor_left.polarity = "normal"
        self.motor_right.polarity = "normal"

        self.motor_left.duty_cycle = 0
        self.motor_right.duty_cycle = 0

        # mode control
        # 0 for pos. 1 for alttitude
        self.mode_control_flag = 0


        self.motor_left.enable()
        self.motor_right.enable()

        #----------Execute Parameter----------------
        self.throttle = 0
        self.steer = 0

        #----------Measure Parameter----------------
        self.yaw = None

        #----------Control Parameter--------------
        self.steer_pid = PID(0.05, 0.1)
        self.speed_pid = PID(0.8, 0.02)
        self.target_yaw = 0
        self.target_speed = 0

        self.target_pos = None
        self.cur_pos = None

        # ==== ros ====
        # pos
        self.pos_pub = rospy.Subscriber(
            "/nlink_linktrack_nodeframe2",
            LinktrackNodeframe2,
            self.on_pos_data
        )

        self.alttitude_sub = rospy.Subscriber(
            "/pi_car/mpu6050/yaw", Float32, self.on_alttitude_data
        )

        self.control_task_timer = rospy.Timer(
            rospy.Duration(0.05),
            self.control_task
        )

        # services
        srv_yaw = rospy.Service(
            "/pi_car/controller/set_yaw",
            String,
            self.handle_set_target_yaw
        )

        srv_brake = rospy.Service(
            "/pi_car/controller/brake",
            Empty,
            self.handle_brake
        )

        srv_speed = rospy.Service(
            "/pi_car/controller/set_speed",
            String,
            self.handle_set_speed
        )

        srv_pos = rospy.Service(
            "/pi_car/controller/set_pos",
            String,
            self.handle_set_pos
        )

        srv_mode_controll = rospy.Service(
            "/pi_car/controller/control_mode",
            String,
            self.handle_set_control_mode
        )

        srv_mode_controll = rospy.Service(
            "/pi_car/controller/set_dyaw",
            String,
            self.handle_set_target_dyaw
        )

    def handle_set_control_mode(self, req: StringRequest):
        self.mode_control_flag = int(req.input)

        return StringResponse(req.input)


    def handle_set_target_yaw(self, req: StringRequest):
        self.target_yaw = float(req.input)
        return StringResponse(req.input)


    def handle_set_target_dyaw(self, req: StringRequest):
        self.target_yaw =self.yaw + float(req.input)
        return StringResponse(req.input)

    def handle_set_speed(self, req: StringRequest):
        speed_ratio = float(req.input)
        if speed_ratio >= 0.8:
            speed_ratio = 0.8

        self.throttle = speed_ratio

        return StringResponse(req.input)


    def handle_brake(self, req: EmptyRequest):
        self.throttle = 0
        # self.lock()

        return EmptyResponse()
    

    def handle_set_pos(self, req: StringRequest):
        self.target_pos = json.loads(req.input)
        return StringResponse(req.input)


    def lock(self):
        self.motor_left.duty_cycle = 0
        self.motor_right.duty_cycle = 0
        self.motor_left.disable()
        self.motor_right.disable()


    def on_alttitude_data(self, data: Float32):
        self.yaw = data.data


    def control_task(self, timer):
        if self.target_pos is None or self.cur_pos is None or self.yaw is None:
            return
        
        # -1 < throttle < 1
        self.speed_ctrl()
        self.steer_ctrl()

        # print("thro", self.throttle, self.mode_control_flag)

        self.motor_left.set_throttle(limit_range(self.throttle + self.steer, -self.MAX_THRO, self.MAX_THRO))
        self.motor_right.set_throttle(limit_range(self.throttle - self.steer, -self.MAX_THRO, self.MAX_THRO))

        # print("thro", self.throttle, self.steer)

    def steer_ctrl(self):
        if self.mode_control_flag == 0:
            cur_pos = np.array(self.cur_pos[0:2])
            target_pos = np.array(self.target_pos)

            dist = np.linalg.norm(target_pos - cur_pos)

            d_pos = target_pos - cur_pos
            self.target_yaw = math.degrees(math.atan2(d_pos[0], d_pos[1]))

            d_yaw = self.target_yaw - self.yaw
        
            if abs(self.get_diff(d_yaw)) > 30 and dist > 0.3:
                self.throttle = 0

                # print(cur_pos, target_pos, dist, d_yaw)

                # print("tyaw", self.target_yaw)
            self.steer_pid.update(self.get_diff(d_yaw))

            self.steer = limit_range(self.steer_pid.out, -self.MAX_STEER, self.MAX_STEER)

            if dist < 0.1:
                self.steer = 0

            if dist < 0.3:            
                self.steer /= 2

        else:
            d_yaw = self.target_yaw - self.yaw
            self.steer_pid.update(self.get_diff(d_yaw))

            self.steer = limit_range(self.steer_pid.out, -self.MAX_STEER, self.MAX_STEER)






    def speed_ctrl(self):
        if self.mode_control_flag == 0:
            cur_pos = np.array(self.cur_pos[0:2])
            target_pos = np.array(self.target_pos)
            dist = np.linalg.norm(target_pos - cur_pos)

            self.speed_pid.update(dist)
            self.throttle = limit_range(self.speed_pid.out, -self.MAX_THRO, self.MAX_THRO)
        


    def get_diff(self, diff):
        while diff > 180:
            diff -= 360
        
        while diff < -180:
            diff += 360

        return diff
    
    def on_pos_data(self, data: LinktrackNodeframe2):
        self.cur_pos = data.pos_3d
        # print(self.cur_pos)


def limit_range(value, minimum, maximum):
    return max(min(value, maximum), minimum)



if __name__ == "__main__":
    try:
        rospy.init_node("picar_controller")

        controller = ControlNode()

        rospy.spin()

    finally:
        controller.lock()